#ifndef ControlRobotTask_H
#define ControlRobotTask_H


//!Includes

#include <stdio.h>
#include <stdlib.h>
#include <iostream>
#include "../algorithms/RobotConection.h"
#include "../algorithms/PBVScontrol.h"
#include "../algorithms/IBVScontrol.h"
#include "../algorithms/Target.h"
#include "../driver/DriverMatrix.h"

//!Defines
#define PBVS 1
#define IBVS 2

//!Global Functions


namespace tasks
{

/**
 * class ControlRobotTask
 *
 * Autor:David Bevilaqua
 *
 * DESCRIPTION:
 *
 */

class ControlRobotTask
{
public:

	//!Enums



	//!Constructors/Destructors

	/**
	 * Empty Constructor
	 */
	ControlRobotTask (int argc,char **argv);

	/**
	 * Empty Destructor
	 */

	~ControlRobotTask ( );

	//!Public Attributes




	//!Public Accessor Methods
	ArClientBase* GetArClient ( );
	void SetArClient (ArClientBase *Client );
	void SetTargetVector (std::vector <algorithms::Target> TargetVector);
	bool GetAchieveTarget();
	bool GetAlign();
	void SetAlign(bool val);
	//!Public Methods
	void SetVelWheelRight (int VelRight);
	void SetVelWheelLeft (int VelLeft );
	void SetLinearVel (int VelLinear);
	void SetAchieveTarget (bool val);
	void SetRotVel (int VelRot );
	void SetHeading (int Heading);
	void Move (int distance);
	void StopRobot ( );
	void LostTarget (bool lost);
	void ConectWithRobot ( );
	void Execute(int val);

protected:

	//!Protected Attributes



	//!Protected Methods



private:

	//!Private Attributes
	char** m_argv;
	int  m_argc;
	algorithms::RobotConection m_RobotConection;
	algorithms::PBVScontrol m_PBVScontrol;
	algorithms::IBVScontrol m_IBVScontrol;
	driver::DriverMatrix m_DriverMatrix;
	ArClientBase *m_pClient;
	std::vector <algorithms::Target> m_TargetVector;
	bool m_bAlign;
	bool m_bLost;
	bool m_bAchieveTarget;
	int m_iLastVelLinear;
	int m_iLastVelRot;
	//!Private Methods
	void AlignAxis();
	void initAttributes ( );

};

};

#endif
